package br.ufrn.dca.controlelrl;

import java.util.ArrayList;

import Jama.EigenvalueDecomposition;
import Jama.Matrix;

/**
 * 
 * @author Rafael Holanda
 *
 */
public class Observador {

	private Matrix wo;
    private Matrix G;
    private Matrix L;
    private Matrix H;
    private Matrix C;
    private Matrix xk;
    
	public Observador() {
		
		double[][] array = {{-152.3306, 153.3272}, {1, 0}};
        //Matriz de observabilidade(tem que ter rank n)
        this.wo = new Matrix(array);
        double[][] Gaux = {{0.993234019752, 0}, {0.00674303920523, 0.993234019752}};
        //Matriz G
        this.G = new Matrix(Gaux);
        double[][] Hawx = {{0.0295427901803}, {0.000100169137108}};
        //Matriz H
        this.H = new Matrix(Hawx);
        double[][] cawx = {{0}, {1}};
        this.C = new Matrix(cawx);
        this.C = this.C.transpose();
        double [][] xkawx = {{0},{0}};
        this.xk = new Matrix(xkawx);

	}

    public Matrix getL() {
        return L;
    }

    public void setL(Matrix L) {
        this.L = L;
    }

    public Matrix retornarL(double[] p1, double[] p2) {
        //Fazer calculo da equation
        double[] equation = new double[3];
//        equation[0] = p1[0] * p2[0];
//        equation[1] = p1[0] * p2[1] + p1[1] * p2[0];
//        equation[2] = p1[1] * p2[1] + p1[2] * p2[2] * (-1);
        
        equation[0] = 1;
        equation[1] = p2[0] + p1[0];
        equation[2] = p1[0] * p2[0] + p1[1] * p2[1] * (-1);
        //Calculando qc
        Matrix G2 = this.G.times(this.G);//G*G
        Matrix G1 = this.G.times(equation[1]);//G*...
        Matrix G0 = Matrix.identity(2, 2).times(equation[2]);//I*...
        Matrix qc = G2.plus(G1.plus(G0));//G*G + ...*G + ...*I
        double[][] x = {{0}, {1}};
        Matrix y = new Matrix(x);
        //Calculando L
        this.L = qc.times(wo.times(y));
        return L;
    }

    //Primeira linha polo 1, Segunda linha polo 2
    public double[][] retornarPolos(Matrix L) {
        this.L=L;
        Matrix parcial1 = this.L.times(C);
        Matrix parcial2 = G.minus(parcial1);
        EigenvalueDecomposition parcial3 = parcial2.eig();
        double[] temp1 = parcial3.getRealEigenvalues();
        double[] temp2 = parcial3.getImagEigenvalues();
        double polos[][] = {temp1, temp2};
        return polos;
    }

    public ArrayList<Matrix> previsao(double x2,double u){
            Matrix yk = C.times(this.xk);
            Matrix Gxk = this.G.times(this.xk);
            double y = yk.get(0,0);
            Matrix Ly = this.L.times((x2-y));
            Matrix Huk = this.H.times(u);
            Matrix GpL = Gxk.plus(Ly);
            this.xk = GpL.plus(Huk);
            ArrayList<Matrix> result = new ArrayList<Matrix>();
            result.add(this.xk);
            result.add(yk);
            return result;
    }
}
